#include "TwoTensor.h"

#include <teem/ten.h>


CTwoTensor::CTwoTensor(void)
{
	m_pTwoTensor = NULL;
	m_w = m_h = m_d = m_n = 0;
}


CTwoTensor::~CTwoTensor(void)
{
	SafeDeleteArray(m_pTwoTensor);
	m_w = m_h = m_d = m_n = 0;
}


int CTwoTensor::OpenTwoTensorFile(const char *pathname, FILE *fp)
{
	if (pathname == NULL)
		return -1;

	fprintf(fp, "Reading Two Tensor file %s ...", pathname);

	Nrrd *dti = nrrdNew();
	if (nrrdLoad(dti, pathname, NULL)) {
		fprintf(fp, "cannot load file %s:\n%s\n", pathname, biffGetDone(NRRD));
		return -2;			// cannot load file
	}

	m_n = (uint)(dti->axis[0].size);
	m_w = (uint)(dti->axis[1].size);
	m_h = (uint)(dti->axis[2].size);
	m_d = (uint)(dti->axis[3].size);

	if (m_n != 14) {
		fprintf(fp, "Not a Two Tensor file!\n");
		return -3;
	}

	m_pTwoTensor = new float[m_w*m_h*m_d*m_n];
	memcpy(m_pTwoTensor, dti->data, sizeof(float)*m_w*m_h*m_d*m_n);
	nrrdNuke(dti);

	fprintf(fp, "Success.\n");
	fprintf(fp, "Width: %d, Height: %d, Depth: %d.\n", 
		m_w, m_h, m_d);
	return 0;
}

int CTwoTensor::SaveTwoTensorFile(const char *pathname, FILE *fp)
{
	if (pathname == NULL || m_pTwoTensor == NULL)
		return -1;

	return 0;
}


void CTwoTensor::GetTwoTensor(const uint x, const uint y, const uint z, float *twoTensor) const
{
	uint index = ((z * m_h + y) * m_w + x) * m_n;
	for (uint i = 0; i < m_n; ++i) {
		twoTensor[i] = m_pTwoTensor[index+i];
	}
}
void CTwoTensor::GetTwoTensor(const uint index, float *twoTensor) const
{
	for (uint i = 0; i < m_n; ++i) {
		twoTensor[i] = m_pTwoTensor[index*m_n+i];
	}
}

void CTwoTensor::GetTensorA(const uint x, const uint y, const uint z, float *ten) const
{
	uint index = ((z * m_h + y) * m_w + x) * m_n;
	for (uint i = 0; i < 7; ++i) {
		ten[i] = m_pTwoTensor[index+i];
	}
}

void CTwoTensor::GetTensorA(const uint index, float *ten) const
{
	for (uint i = 0; i < 7; ++i) {
		ten[i] = m_pTwoTensor[index*m_n+i];
	}
}

void CTwoTensor::GetTensorB(const uint x, const uint y, const uint z, float *ten) const
{
	uint index = ((z * m_h + y) * m_w + x) * m_n;
	for (uint i = 7; i < m_n; ++i) {
		ten[i-7] = m_pTwoTensor[index+i];
	}
}

void CTwoTensor::GetTensorB(const uint index, float *ten) const
{
	for (uint i = 7; i < m_n; ++i) {
		ten[i-7] = m_pTwoTensor[index*m_n+i];
	}
}

void CTwoTensor::Tensor2Direction(const float *dti, CVector3F *dir) const
{
	/* using teem to compute the eigenvector from the 2nd order tensor */
	double m[9], eigenValues[3], eigenVectors[9], trc, iso[9];
	int ret;

	TEN_T2M(m, dti);
	trc = ELL_3M_TRACE(m)/3.0;
	ELL_3M_SCALE_SET(iso, -trc, -trc, -trc);
	ELL_3M_ADD2(m, m, iso);

	ret = ell_3m_eigensolve_d(eigenValues, eigenVectors, m, AIR_TRUE);

	if (ell_cubic_root_single_double == ret) {
		/* this was added to fix a stupid problem with very nearly
		isotropic glyphs, used for demonstration figures */
		if (eigenValues[0] == eigenValues[1]) {
			ELL_3V_CROSS(eigenVectors+6, eigenVectors+0, eigenVectors+3);
		} else {
			ELL_3V_CROSS(eigenVectors+0, eigenVectors+3, eigenVectors+6);
		}
	}

	dir->m_x = (float)(eigenVectors[0]);
	dir->m_y = (float)(eigenVectors[1]);
	dir->m_z = (float)(eigenVectors[2]);
}


float CTwoTensor::Tensor2FA(const float *dti) const
{
	float t[6];
	t[0] = dti[1]; t[1] = dti[2]; t[2] = dti[3]; t[3] = dti[4]; t[4] = dti[5]; t[5] = dti[6];

	float mean = (t[0] + t[1] + t[2] + t[3] + t[4] + t[5]) / 6.0f;
	t[0] = t[0] / mean;
	t[1] = t[1] / mean;
	t[2] = t[2] / mean;
	t[3] = t[3] / mean;
	t[4] = t[4] / mean;
	t[5] = t[5] / mean;

	float cross = t[1] * t[1] + t[2] * t[2] + t[4] * t[4];
	float j2 = t[0] * t[3] + t[3] * t[5] + t[5] * t[0] - cross;
	float j4 = t[0] * t[0] + t[3] * t[3] + t[5] * t[5] + 2.0f * cross;
	float fa = sqrtf((j4-j2)/j4);
	return fa;
}